﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    public class System_Impreza : System_SimplifiedPlanarPhysicalRobot
    {
        private double g = 9.80665;     //m/s/s
        private double m = 1450;   //kg
        private double I_z = 2740;   //kg m2
        private double l_F = 1.3;    //m
        private double l_R = 1.4;    //m
        private double r = 0.3;    //m
        private double I_F = 1.8;    //kg m2
        private double I_R = 1.8;    //kg m2
        private double h = 0.4;    //m
        private double B = 7;
        private double C = 1.6;
        private double D = 0.52;
        //private double C_dl = 60 / 180 * Math.PI;     //rad
        //private double C_Facc = 1000;   //Nm
        //private double C_Fbrk = 700;    //Nm
        //private double C_Racc = 0;      //Nm
        //private double C_Rbrk = 700;    //Nm

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double dl = ctrl[0]; // dl is the steering angle of the front wheel
            double T_F = ctrl[1];// T_F front wheel torque
            double T_R = ctrl[2];// T_R rear wheel torque

            double x = q[0];
            double y = q[1];
            double fi = q[2];
            double xd = q[3];
            double yd = q[4];
            double fid = q[5];
            double w_F = q[6];
            double w_R = q[7];

            double V = Math.Sqrt(xd * xd + yd * yd);
            double beta = Math.Atan2(yd, xd) - fi;
            double V_Fx = V * Math.Cos(beta - dl) + fid * l_F * Math.Sin(dl);
            double V_Fy = V * Math.Sin(beta - dl) + fid * l_F * Math.Cos(dl);
            double V_Rx = V * Math.Cos(beta);
            double V_Ry = V * Math.Sin(beta) - fid * l_R;

            double s_Fx = (V_Fx - w_F * r) / (w_F * r);
            double s_Fy = V_Fy / (w_F * r);
            double s_Rx = (V_Rx - w_R * r) / (w_R * r);
            double s_Ry = V_Ry / (w_R * r);
            double s_F = Math.Sqrt(s_Fx * s_Fx + s_Fy * s_Fy);
            double s_R = Math.Sqrt(s_Rx * s_Rx + s_Ry * s_Ry);

            double mu_F__s_F = D * Math.Sin(C * Math.Atan(B * s_F));
            double mu_R__s_R = D * Math.Sin(C * Math.Atan(B * s_R));
            double mu_Fx = -s_Fx / s_F * mu_F__s_F;
            double mu_Fy = -s_Fy / s_F * mu_F__s_F;
            double mu_Rx = -s_Rx / s_R * mu_R__s_R;
            double mu_Ry = -s_Ry / s_R * mu_R__s_R;

            double f_Fz = (l_R * m * g - h * m * g * mu_Rx) / (l_F + l_R + h * (mu_Fx * Math.Cos(dl) - mu_Fy * Math.Sin(dl) - mu_Rx));
            double f_Rz = m * g - f_Fz;

            double f_Fx = mu_Fx * f_Fz;
            double f_Fy = mu_Fy * f_Fz;
            double f_Rx = mu_Rx * f_Rz;
            double f_Ry = mu_Ry * f_Rz;

            qd[0] = xd;
            qd[1] = yd;
            qd[2] = fid;
            qd[3] = (f_Fx * Math.Cos(fi + dl) - f_Fy * Math.Sin(fi + dl) + f_Rx * Math.Cos(fi) - f_Ry * Math.Sin(fi)) / m;
            qd[4] = (f_Fx * Math.Sin(fi + dl) + f_Fy * Math.Cos(fi + dl) + f_Rx * Math.Sin(fi) + f_Ry * Math.Cos(fi)) / m;
            qd[5] = ((f_Fy * Math.Cos(dl) + f_Fx * Math.Sin(dl)) * l_F - f_Ry * l_R) / I_z;
            qd[6] = (T_F - f_Fx * r) / I_F;
            qd[7] = (T_R - f_Rx * r) / I_R;
        }

        public double Get_SlipVelocity(double[] q)
        {
            double x = q[0];
            double y = q[1];
            double fi = q[2];
            double xd = q[3];
            double yd = q[4];
            double fid = q[5];
            double w_F = q[6];
            double w_R = q[7];
            double V = Math.Sqrt(xd * xd + yd * yd);
            double beta = Math.Atan2(yd, xd) - fi;
            double V_Ry = V * Math.Sin(beta) - fid * l_R;

            return V_Ry;
        }

        override public void Calculate_q_alias(double[] q, ref double[] q_alias)
        {
            if (q_alias == null)
                q_alias = new double[4];

            q_alias[0] = q[0];
            q_alias[1] = q[1];

            double xd = q[3];
            double yd = q[4];

            q_alias[2] = Math.Sqrt(xd * xd + yd * yd);
            q_alias[3] = q[2];
        }

        override public bool GoalReached(double[] q_test, double[] q_target, double tol)
        {
            double x = q_test[0];
            double y = q_test[1];
            double fi = q_test[2];
            double xd = q_test[3];
            double yd = q_test[4];
            double fid = q_test[5];

            /*
            if (x > 15 && x < 20)
                if(y>1)
                    if (Math.Abs(fi) > Math.PI * 0.9)
                    {
                        double length = Math.Sqrt(xd * xd + yd * yd + fid * fid);
                            if (length <= 3)
                                return true;
                    }
             * */
            if (q_test[0] < -9.5)
                if(q_test[1] > 0)
                    return true;
            return false;
        }

        override public void Integration(RK4Helper pad, double[] q, double[] ctrl, double t_delta, ref double[] q_next)
        {
            base.Integration(pad, q, ctrl, t_delta, ref q_next);
            double PI_double = Math.PI + Math.PI;
            while (q_next[2] > Math.PI)
                q_next[2] -= PI_double;
            while (q_next[2] < -Math.PI)
                q_next[2] += PI_double;
        }
    }
}
